#!/usr/bin/python3
# -*- coding: utf-8 -*-

import time, Tools, math
import B_DQ4 as B
import numpy as np
import EventLog as log

P = B.PoseFlow()

def BlancePose(g=0, h1=20, h2=20, b1=0, b2=0, sp=10,der = [0,0],hand = [0,0]):
    # 重心向前偏移值
    q = 3
    
    # 腰部运动，根据迈步来进行调节
    if b1>8:
        waist = int(b1/2)
    elif b2>8:
        waist = -int(b2/2)
    else:
        waist = 0
    handmid = 0
    
    JDDh1 = {13: h1 + b1 - q-3, 14: h1 * 2, 15: h1 - b1 + q}
    JDDh2 = {23: h2 + b2 - q-3, 24: h2 * 2, 25: h2 - b2 + q}
    JDDg = {12: g+der[0], 16: -g, 22: -g+der[1], 26: g}
    JDD = {}
    JDD.update(JDDh1)
    JDD.update(JDDh2)
    JDD.update(JDDg)
    
    # JDDh = {32:2,42:2,34:40,44:40,31:hand[0]-handmid,41:hand[1]-handmid,72:waist,21:-waist,11:waist}
    print(hand[0],hand[1],hand[0]+15,hand[1]+15)
    JDDh = {32:2,42:2,34:int(hand[0]*0.5+15),44:int(hand[1]*0.5+15),31:hand[0],41:hand[1],71:waist,21:-waist,11:waist,72:0}
    JDD.update(JDDh)
    
    log.logger.info(JDD)
    P.SetPoseFlowDic(JDD, sleeptime=0.05, steps=sp)
    
if __name__ == "__main__":
    
    BlancePose(g=0, h1=20, h2=20, b1=0, b2=0, sp=7)
    stepi = 1
    log.logger.info("The %sth setp : Start walk pose" % stepi)
    BlancePose(g=0, h1=20, h2=20, b1=0, b2=0, sp=7)  # 起步
    time.sleep(10)
    
    print("s")
    BlancePose(g=6, h1=20, h2=20, b1=0, b2=0, sp=10)  # 加速
    BlancePose(g=8, h1=20, h2=20, b1=0, b2=0, sp=10)  # 缓冲
    BlancePose(g=8, h1=35, h2=20, b1=0, b2=0, sp=10,der = [0,3])  # 抬腿
    BlancePose(g=7, h1=21, h2=20, b1=12, b2=0, sp=12,der = [0,3])  # 落腿
    
    BlancePose(g=0, h1=21, h2=17, b1=6, b2=-6, sp=20)  # 加速
    
    BlancePose(g=-7.5, h1=21, h2=14, b1=0, b2=-12, sp=40)  # 缓冲
    BlancePose(g=-7.5, h1=20, h2=35, b1=0, b2=0, sp=10,der = [3,0])  # 抬腿
    BlancePose(g=-6, h1=20, h2=22, b1=0, b2=12, sp=12,der = [3,0])  # 落腿
    # 第一步：起步迈步
    # while True:
    #     print("R")
    #     BlancePose(g=-10, h1=20, h2=20, b1=1, b2=-12, sp=7)  # 加速
    #     BlancePose(g=-10, h1=20, h2=35, b1=0, b2=0, sp=7,hand=[10,-10])  # 抬腿
    #     BlancePose(g=-6, h1=20, h2=25, b1=-1, b2=12, sp=7,hand=[15,-15])  # 落腿
    #     BlancePose(g=-6, h1=20, h2=20, b1=-1, b2=12, sp=7,hand=[20,-20])  # 落腿触地
    #     print("L")
    #     BlancePose(g=10, h1=20, h2=20, b1=-12, b2=1, sp=7)  # 加速
    #     BlancePose(g=10, h1=35, h2=20, b1=0, b2=0, sp=7, hand=[-10, 10])  # 抬腿
    #     BlancePose(g=6, h1=25, h2=20, b1=12, b2=-1, sp=7, hand=[-15, 15])  # 落腿
    #     BlancePose(g=6, h1=20, h2=20, b1=12, b2=-1, sp=7, hand=[-20, 20])  # 落腿触地